#include "can_channel.h"
#include "FreeRTOS.h"
#include "FreeRTOSConfig.h"
#include "bsp_can.h"
#include "bsp_utils.h"
#include "can.h"
#include "cmsis_os2.h"
#include "module_base.h"
#include "semphr.h"
#include "stm32f4xx_hal_can.h"
#include "sys/_stdint.h"

osMutexId_t can_lock = NULL;

/**
 * @description: Configure CAN Filters
 * @return {*}
 * @Date: 2024-01-31 21:48:27
 * @LastEditors: Even
 */
void CanInitFilter() {
  uint32_t FilterValue;
  uint32_t FilterMask;
  uint32_t FilterID;
  CAN_FilterTypeDef CAN_FilterStruct;

  can_lock = osMutexNew(&can_mutex_attr);
  configASSERT(can_lock);

  // Extent and remote frame for collect modules
  // FilterID = (1 << 28);
  FilterID = 1;
  FilterValue = CAN_ID_EXT | CAN_RTR_REMOTE | (FilterID << 3);
  FilterMask = (1 << 1) | (1 << 2) | (1 << 3);
  CAN_FilterStruct.FilterMode = CAN_FILTERMODE_IDMASK;
  CAN_FilterStruct.FilterScale = CAN_FILTERSCALE_32BIT;
  CAN_FilterStruct.FilterActivation = ENABLE;
  CAN_FilterStruct.FilterFIFOAssignment = CAN_FILTER_FIFO1;
  CAN_FilterStruct.FilterMaskIdHigh = (uint16_t)(FilterMask >> 16);
  CAN_FilterStruct.FilterMaskIdLow = FilterMask;
  CAN_FilterStruct.FilterIdHigh = (uint16_t)(FilterValue >> 16);
  CAN_FilterStruct.FilterIdLow = FilterValue;
  CAN_FilterStruct.SlaveStartFilterBank = 24;
  // can2
  CAN_FilterStruct.FilterBank = 24;
  HAL_CAN_ConfigFilter(&hcan2, &CAN_FilterStruct);
  // can1
  CAN_FilterStruct.FilterBank = 0;
  HAL_CAN_ConfigFilter(&hcan1, &CAN_FilterStruct);

  // Extent and data frame for module long pack
  FilterID = 1;
  FilterValue = CAN_ID_EXT | CAN_RTR_REMOTE | (FilterID << 3);
  FilterMask = (1 << 1) | (1 << 2) | (1 << 3);
  CAN_FilterStruct.FilterMode = CAN_FILTERMODE_IDMASK;
  CAN_FilterStruct.FilterScale = CAN_FILTERSCALE_32BIT;
  CAN_FilterStruct.FilterActivation = ENABLE;
  CAN_FilterStruct.FilterFIFOAssignment = CAN_FILTER_FIFO1;
  CAN_FilterStruct.FilterMaskIdHigh = (uint16_t)(FilterMask >> 16);
  CAN_FilterStruct.FilterMaskIdLow = FilterMask;
  CAN_FilterStruct.FilterIdHigh = (uint16_t)(FilterValue >> 16);
  CAN_FilterStruct.FilterIdLow = FilterValue;
  CAN_FilterStruct.SlaveStartFilterBank = 24;
  // can2
  CAN_FilterStruct.FilterBank = 25;
  HAL_CAN_ConfigFilter(&hcan2, &CAN_FilterStruct);
  // can1
  CAN_FilterStruct.FilterBank = 1;
  HAL_CAN_ConfigFilter(&hcan1, &CAN_FilterStruct);

  // Stander and data frame for fix id, 0-31
  FilterID = 0x600;
  FilterValue = CAN_ID_EXT | CAN_RTR_REMOTE | (FilterID << 21);
  FilterMask = (1 << 1) | (1 << 2) | (0x600 << 21);
  CAN_FilterStruct.FilterMode = CAN_FILTERMODE_IDMASK;
  CAN_FilterStruct.FilterScale = CAN_FILTERSCALE_32BIT;
  CAN_FilterStruct.FilterActivation = ENABLE;
  CAN_FilterStruct.FilterFIFOAssignment = CAN_FILTER_FIFO1;
  CAN_FilterStruct.FilterMaskIdHigh = (uint16_t)(FilterMask >> 16);
  CAN_FilterStruct.FilterMaskIdLow = FilterMask;
  CAN_FilterStruct.FilterIdHigh = (uint16_t)(FilterValue >> 16);
  CAN_FilterStruct.FilterIdLow = FilterValue;
  CAN_FilterStruct.SlaveStartFilterBank = 24;
  // can2
  CAN_FilterStruct.FilterBank = 26;
  HAL_CAN_ConfigFilter(&hcan2, &CAN_FilterStruct);
  // can1
  CAN_FilterStruct.FilterBank = 2;
  HAL_CAN_ConfigFilter(&hcan1, &CAN_FilterStruct);
}

/**
 * @description: Can bus send data
 * @param {uint32_t} ID: The ID of the target
 * @param {uint8_t} IDType: The ID Type, standar(IDTYPE_STD) or
 * externsion(IDTYPE_EXT)
 * @param {uint8_t} PortNum: The can bus port number, 1 or 2
 * @param {uint8_t} FrameType: The frame type,etc remote control or data
 * @param {uint8_t} DataLen: The count of the data to be send
 * @param {uint8_t} *pData: The pointer to the data
 * @return {*} true if success, or else false
 * @Date: 2024-01-28 22:36:42
 * @LastEditors: Even
 */
uint32_t Canbus::CanSendPacked(uint32_t ID, uint8_t IDType, uint8_t PortNum,
                               uint8_t FrameType, uint8_t DataLen,
                               uint8_t *pData) {
  CAN_TxHeaderTypeDef TxMesHeader;
  uint8_t retry;
  uint32_t tmptick;
  uint32_t regtsr;
  uint32_t can_esr = 0;

  BaseType_t ret = pdFAIL;

  if (DataLen > 8)
    return false;

  if (FrameType == FRAME_DATA) {
    TxMesHeader.RTR = CAN_RTR_DATA;
    TxMesHeader.DLC = DataLen;
    // for (int i = 0; i < DataLen; i++)
    //   TxMesHeader.Data[i] = *pData++;
  } else if (FrameType == FRAME_REMOTE) {
    TxMesHeader.RTR = CAN_RTR_REMOTE;
    TxMesHeader.DLC = 0;
  }
  if (IDTYPE_STDID == IDType) {
    TxMesHeader.IDE = CAN_ID_STD;
    TxMesHeader.StdId = ID;
  } else {
    TxMesHeader.IDE = CAN_ID_EXT;
    TxMesHeader.ExtId = ID;
  }

  retry = 1;
  if (PortNum == 1) {
    while (retry--) {

      if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING)
        ret = osMutexAcquire(can_lock, portMAX_DELAY);

      HAL_CAN_AddTxMessage(&hcan1, &TxMesHeader, pData,
                           (uint32_t *)CAN_TX_MAILBOX0);

      if (ret == pdPASS)
        xSemaphoreGive(can_lock);

      tmptick = millis();
      // while pending
      do {
        if ((millis() - tmptick) > 500)
          break;
        regtsr = CAN1->TSR & (CAN_TSR_TXOK0 | CAN_TSR_RQCP0 | CAN_TSR_TME0);
        can_esr = CAN1->ESR;
        if (regtsr == (CAN_TSR_TXOK0 | CAN_TSR_RQCP0 | CAN_TSR_TME0)) {
          return 0;
        } else if (regtsr == (CAN_TSR_RQCP0 | CAN_TSR_TME0))
          break;
      } while (true);
    }
  } else {
    while (retry--) {
      if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING)
        ret = osMutexAcquire(can_lock, portMAX_DELAY);

      HAL_CAN_AddTxMessage(&hcan2, &TxMesHeader, pData,
                           (uint32_t *)CAN_TX_MAILBOX0);

      if (ret == pdPASS)
        xSemaphoreGive(can_lock);

      tmptick = millis();
      // while pending
      do {
        if ((millis() - tmptick) > 500)
          break;
        regtsr = CAN2->TSR & (CAN_TSR_TXOK0 | CAN_TSR_RQCP0 | CAN_TSR_TME0);
        can_esr = CAN2->ESR;
        if (regtsr == (CAN_TSR_TXOK0 | CAN_TSR_RQCP0 | CAN_TSR_TME0)) {
          return 0;
        } else if (regtsr == (CAN_TSR_RQCP0 | CAN_TSR_TME0))
          break;
      } while (true);
    }
  }
  return can_esr;
}

CanChannel can;

ErrCode CanChannel::Init(CANIrqCallback_t irq_cb) {
  void *tmp = NULL;

  tmp = pvPortMalloc(CAN_MAC_QUEUE_SIZE * 4);
  if (!tmp) {
    return E_NO_MEM;
  }
  mac_id_.Init((int32_t)CAN_MAC_QUEUE_SIZE, (uint32_t *)tmp);

  tmp = pvPortMalloc(CAN_EXT_CMD_QUEUE_SIZE);
  if (!tmp) {
    return E_NO_MEM;
  }
  ext_cmd_.Init((int32_t)CAN_EXT_CMD_QUEUE_SIZE, (uint8_t *)tmp);

  std_cmd_w_ = 0;
  std_cmd_r_ = 0;
  std_cmd_in_q_ = 0;

  for (int i = 0; i < CAN_CH_MAX; i++) {
    lock_[i] = osMutexNew(&can_mutex_attr);
    configASSERT(lock_[i]);
  }

  irq_cb_ = irq_cb;

  HAL_CAN_Start(&hcan1);
  HAL_CAN_Start(&hcan2);
  CanInitFilter();

  return E_SUCCESS;
}

ErrCode CanChannel::Write(CanPacket_t &packet) {
  BaseType_t ret_lock = pdFAIL;
  uint32_t ret_send = 0;

  if (xTaskGetSchedulerState() == taskSCHEDULER_RUNNING)
    ret_lock = osMutexAcquire(lock_[packet.ch], portMAX_DELAY); // 查询可否访问

  switch (packet.ft) {
  case CAN_FRAME_STD_DATA:
    if (packet.length > 8) {
      if (ret_lock == pdPASS)
        xSemaphoreGive(lock_[packet.ch]);

      return E_PARAM;
    }

    ret_send = Canbus::CanSendPacked(packet.id, IDTYPE_STDID, packet.ch + 1,
                                     FRAME_DATA, packet.length, packet.data);

    break;

  case CAN_FRAME_EXT_DATA:
    for (int32_t i = 0; i < packet.length; i += 8) {
      if (packet.length - i > 8)
        ret_send = Canbus::CanSendPacked(packet.id, IDTYPE_EXTID, packet.ch + 1,
                                         FRAME_DATA, 8, packet.data + i);
      else
        ret_send = Canbus::CanSendPacked(packet.id, IDTYPE_EXTID, packet.ch + 1,
                                         FRAME_DATA, packet.length - i,
                                         packet.data + i);
    }
    break;

  case CAN_FRAME_EXT_REMOTE:
    ret_send = Canbus::CanSendPacked(packet.id, IDTYPE_EXTID, packet.ch + 1,
                                     FRAME_REMOTE, 0, 0);
    break;

  case CAN_FRAME_STD_REMOTE:
    ret_send = Canbus::CanSendPacked(packet.id, IDTYPE_STDID, packet.ch + 1,
                                     FRAME_REMOTE, 0, 0);
    break;

  default:
    break;
  }

  if (ret_lock == pdPASS) {
    xSemaphoreGive(lock_[packet.ch]);
  }

  if (!ret_send) {
    // LOG_I("[CH%u:0x%X] send ok\n", packet.ch + 1, packet.id);
    return E_SUCCESS;
  } else {
    // LOG_I("[CH%u:0x%X] failed to send can packet: 0x%X\n", packet.ch + 1,
    // packet.id, ret_send);
    return E_FAILURE;
  }
}

int32_t CanChannel::Available(CanFrameType ft) {
  switch (ft) {
  case CAN_FRAME_STD_DATA:
    return CAN_STD_CMD_QUEUE_SIZE - std_cmd_in_q_;

  case CAN_FRAME_EXT_DATA:
    return ext_cmd_.Available();

  case CAN_FRAME_EXT_REMOTE:
    return mac_id_.Available();

  default:
    break;
  }

  return 0;
}

int32_t CanChannel::Read(CanFrameType ft, uint8_t *buffer, int32_t l) {
  int32_t i = 0;

  uint8_t *tmp_pu8;

  if (!buffer) {
    return -E_PARAM;
  }

  switch (ft) {
  case CAN_FRAME_STD_DATA:
    if (std_cmd_in_q_ == 0)
      return 0;

    tmp_pu8 = (uint8_t *)&std_cmd_[std_cmd_r_];

    for (i = 0; i < CAN_STD_CMD_ELEMENT_SIZE; i++) {
      buffer[i] = tmp_pu8[i];
    }

    if (++std_cmd_r_ >= CAN_STD_CMD_QUEUE_SIZE)
      std_cmd_r_ = 0;

    std_cmd_in_q_--;

    return CAN_STD_CMD_ELEMENT_SIZE;

  case CAN_FRAME_EXT_DATA:
    return ext_cmd_.RemoveMulti(buffer, l);

  case CAN_FRAME_EXT_REMOTE:
    return mac_id_.RemoveMulti((uint32_t *)buffer, l);

  default:
    break;
  }

  return 0;
}

void CanChannel::Irq(CanChannelNumber ch, uint8_t fifo_index) {
  int32_t i = 0;
  uint8_t filter_index = 0;

  uint32_t can_id;
  uint8_t id_type;
  uint8_t frame_type;
  uint8_t length;

  CanStdDataFrame_t std_data_frame;

  // read data
  switch (ch) {
  case CAN_CH_1:
    filter_index =
        Canbus::CanbusParseData(&hcan1, &can_id, &id_type, &frame_type,
                                std_data_frame.data, &length, fifo_index);
    break;

  case CAN_CH_2:
    filter_index =
        Canbus::CanbusParseData(&hcan2, &can_id, &id_type, &frame_type,
                                std_data_frame.data, &length, fifo_index);
    break;

  default:
    return;
  }

  // standard data frame
  if (fifo_index == 0) {
    if (id_type == IDTYPE_STDID && !filter_index) {
      std_data_frame.id.val = (uint16_t)can_id;
      std_data_frame.id.bits.length = length & 0x1F;

      // check if we have callback for this message id
      // if callback return true, indicates message is handled
      if (irq_cb_ && irq_cb_(std_data_frame)) {
        return;
      }

      // if no callback, enqueue the data just received
      if (std_cmd_in_q_ < CAN_STD_CMD_QUEUE_SIZE) {
        std_cmd_[std_cmd_w_].id = std_data_frame.id;
        // save data field
        for (i = 0; i < length; i++) {
          std_cmd_[std_cmd_w_].data[i] = std_data_frame.data[i];
        }
        if (++std_cmd_w_ >= CAN_STD_CMD_QUEUE_SIZE)
          std_cmd_w_ = 0;
        std_cmd_in_q_++;
      }
    }

    return;
  }

  // first bit indicates main controller or modules
  can_id &= 0xFFFFFFFE;

  // extended data frame
  if (id_type == IDTYPE_EXTID && filter_index) {
    // if (std_data_frame.data[0] == 0xAA &&
    //     std_data_frame.data[1] == 0x55) {
    //   // to tell upper level the sender, put can_id in following
    //   ext_cmd_.InsertMulti((uint8_t *)&can_id, 4);
    // }

    ext_cmd_.InsertMulti(std_data_frame.data, length);

    return;
  }

  // extended remote frame
  if (id_type == IDTYPE_EXTID && !filter_index) {
    // low 29 bits is actual module MAC
    // we add channel number in the bit[29], to tell upper level

    // bit[29] == 0 indicates CAN1
    // bit[29] == 1 indicates CAN2
    if (ch == CAN_CH_1)
      can_id &= ~(1 << 29);
    else
      can_id |= (1 << 29);

    mac_id_.InsertOne(can_id);
    return;
  }
}

namespace Canbus {
CAN_RxHeaderTypeDef *pRxHeader = NULL; // 暂存收到的数据帧头
}

/**
 * @description: Can bus parse data for ISR
 * @param {CAN_HandleTypeDef} *hcan: Can handle
 * @param {uint32_t} *ID: The pointer to save the ID
 * @param {uint8_t} *IDType: CAN IDTYPE_STDID or IDTYPE_EXTID
 * @param {uint8_t} *FrameType: The pointer to save the frame type,etc remote
 * control or data
 * @param {uint8_t} *pData: The pointer to the data
 * @param {uint8_t} *Len: The pointer to the Len
 * @param {uint8_t} FIFONum: The FIFO number
 * @return {*} The filter index
 * @Date: 2024-01-28 22:27:33
 * @LastEditors: Even
 */
uint8_t Canbus::CanbusParseData(CAN_HandleTypeDef *hcan, uint32_t *ID,
                                uint8_t *IDType, uint8_t *FrameType,
                                uint8_t *pData, uint8_t *Len, uint8_t FIFONum) {
  uint8_t IDE;
  uint8_t FMI;
  HAL_CAN_GetRxMessage(hcan, FIFONum, pRxHeader, pData);
  IDE = pRxHeader->IDE;
  if (IDE == CAN_ID_STD) {
    *ID = pRxHeader->StdId;
    *IDType = IDTYPE_STDID;
  } else {
    *ID = pRxHeader->ExtId;
    *IDType = IDTYPE_EXTID;
  }
  *FrameType = pRxHeader->RTR;       // 区分数据帧/远程帧
  *Len = pRxHeader->DLC;             // 数据长度0~8，远程帧为0
  FMI = pRxHeader->FilterMatchIndex; // 筛选器匹配索引
  return FMI;
}

extern "C" {

void __irq_can1_tx(void) {}

void __irq_can1_rx0(void) { can.Irq(CAN_CH_1, 0); }

void __irq_can1_rx1(void) { can.Irq(CAN_CH_1, 1); }

void __irq_can1_sce(void) {}

void __irq_can2_tx(void) {}

void __irq_can2_rx0(void) { can.Irq(CAN_CH_2, 0); }

void __irq_can2_rx1(void) { can.Irq(CAN_CH_2, 1); }

void __irq_can2_sce(void) {}
}